from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler
from launch.conditions import IfCondition, UnlessCondition
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression, PathJoinSubstitution
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

import os

def generate_launch_description():
    # 获取包路径
    bc_pkg_share = get_package_share_directory('ball_collector')
    bringup_pkg_share = get_package_share_directory('ball_collector_bringup')
    
    # 配置参数文件
    detection_params = os.path.join(bc_pkg_share, 'config', 'detection_params.yaml')
    robot_params = os.path.join(bc_pkg_share, 'config', 'robot_params.yaml')
    
    # RViz配置
    rviz_config = os.path.join(bc_pkg_share, 'config', 'rviz', 'visualization.rviz')
    
    # 声明启动参数
    camera_source = LaunchConfiguration('camera_source')
    use_yolo = LaunchConfiguration('use_yolo')
    robot_name = LaunchConfiguration('robot_name')
    use_rviz = LaunchConfiguration('use_rviz')
    use_simulation = LaunchConfiguration('use_simulation')
    
    # 参数声明
    declare_camera_source = DeclareLaunchArgument(
        'camera_source',
        default_value='0',
        description='Camera source (index or device path)'
    )
    
    declare_use_yolo = DeclareLaunchArgument(
        'use_yolo',
        default_value='true',
        description='Whether to use YOLO for detection'
    )
    
    declare_robot_name = DeclareLaunchArgument(
        'robot_name',
        default_value='ball_collector_robot',
        description='Robot name for TF and namespaces'
    )
    
    declare_use_rviz = DeclareLaunchArgument(
        'use_rviz',
        default_value='true',
        description='Whether to start RViz'
    )
    
    declare_use_simulation = DeclareLaunchArgument(
        'use_simulation',
        default_value='false',
        description='Whether to use simulation instead of real hardware'
    )
    
    # 相机启动节点
    camera_node = Node(
        package='v4l2_camera',
        executable='v4l2_camera_node',
        name='camera_node',
        namespace=robot_name,
        parameters=[{
            'video_device': camera_source,
            'image_width': 640,
            'image_height': 480,
            'pixel_format': 'YUYV',
            'camera_frame_id': ['camera_', robot_name, '_link']
        }],
        remappings=[
            ('image_raw', 'camera/image_raw')
        ],
        condition=UnlessCondition(use_simulation)
    )
    
    # 模拟相机节点（使用仿真时）
    sim_camera_node = Node(
        package='ball_collector',
        executable='sim_camera_node',  # 假设存在这个模拟相机节点
        name='sim_camera_node',
        namespace=robot_name,
        parameters=[{
            'publish_rate': 30.0,
            'simulate_balls': True,
            'camera_frame_id': ['camera_', robot_name, '_link']
        }],
        remappings=[
            ('image_raw', 'camera/image_raw')
        ],
        condition=IfCondition(use_simulation)
    )
    
    # 球检测节点
    detection_node = Node(
        package='ball_collector',
        executable='detection_node',
        name='detection_node',
        namespace=robot_name,
        parameters=[
            detection_params,
            {'camera_topic': ['/', robot_name, '/camera/image_raw']},
            {'use_yolo': use_yolo}
        ],
        remappings=[
            ('detected_ball', 'vision/detected_ball'),
            ('detections', 'vision/detections'),
            ('detection_image', 'vision/detection_image'),
            ('ball_markers', 'vision/ball_markers')
        ]
    )
    
    # 策略节点
    strategy_node = Node(
        package='ball_collector',
        executable='strategy_node',
        name='strategy_node',
        namespace=robot_name,
        parameters=[robot_params],
        remappings=[
            ('detected_ball', 'vision/detected_ball'),
            ('cmd_vel', 'control/cmd_vel'),
            ('robot_state', 'status/robot_state')
        ]
    )
    
    # 硬件接口节点（真实硬件）
    hardware_node = Node(
        package='ball_collector',
        executable='hardware_interface',
        name='hardware_interface',
        namespace=robot_name,
        parameters=[{
            'auto_discover': True,
            'baud_rate': 115200,
            'max_linear_speed': 0.5,
            'max_angular_speed': 1.0
        }],
        remappings=[
            ('cmd_vel', 'control/cmd_vel'),
            ('hardware_status', 'status/hardware')
        ],
        condition=UnlessCondition(use_simulation)
    )
    
    # 模拟硬件接口节点（使用仿真时）
    sim_hardware_node = Node(
        package='ball_collector',
        executable='sim_hardware_interface',  # 假设存在这个模拟硬件接口节点
        name='sim_hardware_interface',
        namespace=robot_name,
        parameters=[{
            'max_linear_speed': 0.5,
            'max_angular_speed': 1.0,
            'update_rate': 50.0,
            'simulate_gripper': True
        }],
        remappings=[
            ('cmd_vel', 'control/cmd_vel'),
            ('hardware_status', 'status/hardware')
        ],
        condition=IfCondition(use_simulation)
    )
    
    # TF静态变换发布器 - 相机到基座的变换
    tf_camera_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='camera_link_broadcaster',
        arguments=['0.1', '0', '0.1', '0', '0', '0', 
                  [robot_name, '_base_link'], 
                  ['camera_', robot_name, '_link']]
    )
    
    # TF静态变换发布器 - 基座到大地的变换
    tf_base_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_link_broadcaster',
        arguments=['0', '0', '0.05', '0', '0', '0', 
                  'map', 
                  [robot_name, '_base_link']]
    )
    
    # RViz节点
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config],
        condition=IfCondition(use_rviz)
    )
    
    # 创建启动描述
    ld = LaunchDescription()
    
    # 添加声明参数
    ld.add_action(declare_camera_source)
    ld.add_action(declare_use_yolo)
    ld.add_action(declare_robot_name)
    ld.add_action(declare_use_rviz)
    ld.add_action(declare_use_simulation)
    
    # 添加节点
    ld.add_action(camera_node)
    ld.add_action(sim_camera_node)
    ld.add_action(detection_node)
    ld.add_action(strategy_node)
    ld.add_action(hardware_node)
    ld.add_action(sim_hardware_node)
    ld.add_action(tf_camera_node)
    ld.add_action(tf_base_node)
    ld.add_action(rviz_node)
    
    return ld